///////////////////////////////////////////////////////////////////////////////
// Name:        ilmatrixoperations.cpp
// Purpose:     Operations on matrices
// Created:     2006
// Copyright:   (c) 2006, Ivan Laptev, TT Solutions
// License:     GPL, Please contact us for commercial license.
//              http://www.tt-solutions.com/
///////////////////////////////////////////////////////////////////////////////
#include "objectdet/ilmatrixoperations.h"

//************************************************************************
// ilEuclidDist
//   Computes Euclidean distance between sets of points in src1 and src2.
//   src1 is m1*n array; src2 is m2*n array where m1 and m2 are
//   the number of points in each set and n is the dimensionality
//   of the Euclidean space. The result dst is an m1*m2 matrix
//   with distances between all pairs of points.
//   If m1=m2=1 Euclidean distance between two vectors is computed.
//************************************************************************
void ilEuclidDist(pMat const& src1, pMat const& src2, pMat dst)
{
    int m1 = src1->rows;
    int m2 = src2->rows;
    int n = src1->cols;
    int type=src1->type;

#if 0 
    //*** matrix version
    pMat ones_nm2 = CreateMat(n, m2, type);
    pMat ones_m1n = CreateMat(m1, n, type);
    pMat src1sq = CreateMat(m1, n, type);
    pMat src2sq = CreateMat(m2, n, type);
    pMat res1 = CreateMat(m1, m2, type);
    pMat res2 = CreateMat(m1, m2, type);

    Multiply(src1, src1, src1sq);
    Multiply(src2, src2, src2sq);

    SetValue(ones_nm2, cvScalar(1.0));
    SetValue(ones_m1n, cvScalar(1.0));

    //*** Euclidean distance with matrix multiplications
    MatrixMultiply(src1sq, ones_nm2, res1);
    GEMM(ones_m1n, src2sq, 1.0, res1, 1, res2, CV_GEMM_B_T);
    GEMM(src1, src2, -2.0, res2, 1, dst, CV_GEMM_B_T);
    PowerMatrix(dst, dst, .5);
#endif

#if 1
    //*** element-wise version
    pMat src1sq = CreateMat(m1, n, type);
    pMat src2sq = CreateMat(m2, n, type);
    Multiply(src1, src1, src1sq);
    Multiply(src2, src2, src2sq);

    float *ptrsrc1sq, *ptrsrc2sq, *ptrdst;
    ptrsrc1sq = Ptr2D<float>(src1sq);
    ptrsrc2sq = Ptr2D<float>(src2sq);
    ptrdst = Ptr2D<float>(dst);
    int indsrc1sq, indsrc2sq, inddst=0;
    float v0, v1, v2, v3, v4, v5, v6, v7, v8, v9, vdst;
    for(int i = 0; i<m1; ++i)
    {
        indsrc1sq = i * n;
        v0 = ptrsrc1sq[indsrc1sq + 0];
        v1 = ptrsrc1sq[indsrc1sq + 1];
        v2 = ptrsrc1sq[indsrc1sq + 2];
        v3 = ptrsrc1sq[indsrc1sq + 3];
        v4 = ptrsrc1sq[indsrc1sq + 4];
        v5 = ptrsrc1sq[indsrc1sq + 5];
        v6 = ptrsrc1sq[indsrc1sq + 6];
        v7 = ptrsrc1sq[indsrc1sq + 7];
        v8 = ptrsrc1sq[indsrc1sq + 8];
        v9 = ptrsrc1sq[indsrc1sq + 9];
        for (int j=0; j<m2; ++j)
        {
    	    inddst = i * m2 + j;
    	    indsrc2sq = j * n;
    	    vdst  = (v0-ptrsrc2sq[indsrc2sq])   * (v0-ptrsrc2sq[indsrc2sq]);
    	    vdst += (v1-ptrsrc2sq[indsrc2sq+1]) * (v1-ptrsrc2sq[indsrc2sq+1]);
    	    vdst += (v2-ptrsrc2sq[indsrc2sq+2]) * (v2-ptrsrc2sq[indsrc2sq+2]);
    	    vdst += (v3-ptrsrc2sq[indsrc2sq+3]) * (v3-ptrsrc2sq[indsrc2sq+3]);
    	    vdst += (v4-ptrsrc2sq[indsrc2sq+4]) * (v4-ptrsrc2sq[indsrc2sq+4]);
    	    vdst += (v5-ptrsrc2sq[indsrc2sq+5]) * (v5-ptrsrc2sq[indsrc2sq+5]);
    	    vdst += (v6-ptrsrc2sq[indsrc2sq+6]) * (v6-ptrsrc2sq[indsrc2sq+6]);
    	    vdst += (v7-ptrsrc2sq[indsrc2sq+7]) * (v7-ptrsrc2sq[indsrc2sq+7]);
    	    vdst += (v8-ptrsrc2sq[indsrc2sq+8]) * (v8-ptrsrc2sq[indsrc2sq+8]);
    	    vdst += (v9-ptrsrc2sq[indsrc2sq+9]) * (v9-ptrsrc2sq[indsrc2sq+9]);
    	    ptrdst[inddst] = vdst;
        }
    }
#endif
}

//*******************************************************************
// ilEuclidNorm
//   normalizes vectors of dim=n in mxn input matrix with the
//   Euclidean norm
//*******************************************************************
void ilEuclidNorm(pMat const& src, pMat dst)
{
    int m = src->rows;
    int n = src->cols;
    int type = src->type;
    pMat ones = CreateMat(n, 1, type);
    pMat res1 = CreateMat(m, n, type);
    pMat norm = CreateMat(m, 1, type);
    SetValue(ones, cvScalar(1.0));

    //*** compute the norm
    Multiply(src, src, res1);
    MatrixMultiply(res1, ones, norm);
    PowerMatrix(norm, norm, .5);

    //*** normalize columns of src
#if 0 //*** matrix version
    pMat normmat = CreateMat(m, n, CV_32FC1);
    pMat onesrow = CreateMat(1, n, CV_32FC1);
    SetValue(onesrow, cvScalar(1.0));
    MatrixMultiply(norm, onesrow, normmat);
    Divide(src, normmat, dst);
#endif

#if 1 // *** column version
    CvMat colmat1 = cvMat(0, 0, 0, 0);
    CvMat colmat2 = cvMat(0, 0, 0, 0);
    for (int i = 0; i<n; ++i)
    {
        cvGetCol(src.get(), &colmat1, i);
        cvGetCol(dst.get(), &colmat2, i);
        Divide(dummyGuard(&colmat1), norm, dummyGuard(&colmat2));
    }
#endif
}

//*********************************************************************
// ilMaxRowFloat
//   Finds max values and corresponding indicies along the rows
//   of an input 2-dim. float mxn matrix. maxvalarr and maxindarr
//   should have size mx1. maxindarr should be of type short.
//*********************************************************************
void ilMaxRowFloat(pMat const& src, pMat maxvalmat, pMat maxindmat, int absflag)
{
    short maxind;
    float maxval,val;
    int m=src->rows, n=src->cols, type=src->type, indsrc;
    float const* ptrsrc = Ptr2D<float const>(src);
    float* ptrmaxval = Ptr2D<float>(maxvalmat);
    short* ptrmaxind = Ptr2D<short>(maxindmat);
    for (int i = 0; i < m; ++i)
    {
        indsrc = i * n;
        maxval = ptrsrc[indsrc];
        maxind = 0;
        for (int j = 0; j < n; ++j)
        {
            val = ptrsrc[indsrc + j];
            if (absflag)
            {
                if(val < 0)
                    val = -val;
            }
            if (val > maxval)
            {
                maxind = j;
                maxval = val;
            }
        }
        ptrmaxval[i] = maxval;
        ptrmaxind[i] = maxind;
    }
}

//*********************************************************************
// ilMaxColFloat
//     Finds max values and corresponding indicies along the columns
//     of an input 2-dim. float mxn matrix. maxvalarr and maxindarr
//     should have size nx1. maxindarr should be of type short.
//*********************************************************************
void ilMaxColFloat(pMat const& src, pMat maxvalmat, pMat maxindmat, int absflag)
{
    short maxind;
    float maxval,val;
    int m=src->rows, n=src->cols, type=src->type, indsrc;
    float const *ptrsrc = Ptr2D<float const>(src);
    float *ptrmaxval = Ptr2D<float>(maxvalmat);
    short *ptrmaxind = Ptr2D<short>(maxindmat);
    for (int i=0; i<n; i++)
    {
        indsrc = i;
        maxval = ptrsrc[indsrc];
        if (absflag)
        {
            if (maxval < 0)
                maxval = -maxval;
        }
        maxind = 0;
        for (int j = 0; j < m; ++j)
        {
            val = ptrsrc[indsrc + j * n];
            if (absflag)
            {
                if (val<0)
                    val =- val;
            }
            if (val > maxval)
            {
                maxind = j;
                maxval = val;
            }
        }
        ptrmaxval[i] = maxval;
        ptrmaxind[i] = maxind;
    }
}

//*********************************************************************
// ilMaxProjectFloat
//     Projects columns of mxd matrix stcarr2 onto rows of nxm matrix srcarr1
//     and finds max values and corresponding indicies along the columns
//     of a resulting nxd projection matrix. Output matrices maxvalarr and maxindarr
//     should have size dx1. maxindarr should be of type short.
//*********************************************************************
void ilMaxProjectFloat(pMat const& src1, pMat const& src2, pMat maxvalmat, pMat maxindmat)
{
    short maxind;
    float maxval, val;
    int m = src1->rows, n = src1->cols, d = src2->cols, type = src1->type, indsrc1, indsrc2;
    float *ptrsrc1 = Ptr2D<float>(src1);
    float *ptrsrc2 = Ptr2D<float>(src2);
    float *ptrmaxval = Ptr2D<float>(maxvalmat);
    short *ptrmaxind = Ptr2D<short>(maxindmat);

    for (int i = 0; i < d; ++i)
    {
        indsrc2 = i;
        maxval = 0;
        for (int j = 0; j < m; ++j)
        {
            indsrc1 = j * n;
            val = 0.0;
            for (int k = 0; k < n; ++k)
            {
                val += ptrsrc2[indsrc2 + k * d] * ptrsrc1[indsrc1 + k];
            }
     	    if ((val > maxval) || (j == 0))
            {
                maxind = j;
                maxval = val;
            }
        }
        ptrmaxval[i] = maxval;
        ptrmaxind[i] = maxind;
    }
}

//**************************************************************
// ilMatTestFun
//     performs tests of matrix functions:
//     ilEuclidDist, ilEuclidNorm, ilMinAtDim
//***************************************************************
void ilMatFunTest()
{
#if 0 //*** testing ilEuclidNorm (comparison with matlab)
    std::string matfnamein = "C:/temp/matdata3.xml";
    std::string matfnameout = "C:/temp/matdata4.xml";

	pFileStorage fsr = OpenFileStorage(matfnamein, RWM_Read);
    pMat mat1 = ReadMatrixFromFile(fsr, "matrix1");

    //*** normalize
    pMat mat2 = CreateMat(mat1->rows, mat1->cols, mat1->type);
    ilEuclidNorm(mat1, mat2);

    //*** Save result in xml
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
    WriteMatrixToFile(fsw, "matrix2", mat2);

    /*
    % Matlab test code
    m1=rand(10,5);
    xmlsavematrices('C:/temp/matdata3.xml',{m1},{'matrix1'});
        %%% run ilMatFunTest();
    [matval,matname]=xmlreadmatrices('c:/temp/matdata4.xml');
    enorm=euclidnorm(m1);
    err=mean(abs(enorm(:)-matval{1}(:)))
    */
#endif

#if 0 //*** testing ilEuclidDist (comparison with matlab)
    std::string matfnamein = "C:/temp/matdata1.xml";
    std::string matfnameout = "C:/temp/matdata2.xml";
    // we don't own the matrices below -- file storage does
    pFileStorage fsr = OpenFileStorage(matfnamein, RWM_Read);
    pMat mat1 = ReadMatrixFromFile(fsr, "matrix1");
    pMat mat2 = ReadMatrixFromFile(fsr, "matrix2");

    //*** compute Euclidean distance
    pMat mat3 = CreateMat(mat1->rows, mat2->rows, mat1->type);
    ilEuclidDist(mat1, mat2, mat3);

    //*** Save result in xml
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
    WriteMatrixToFile(fsw, "matrix3", mat3);

//    cvReleaseMat(&mat1); // TODO do we have to?
//    cvReleaseMat(&mat2); // TODO do we have to?

    /*
        % Matlab test code
        m1=rand(10,5); m2=rand(20,5);
        xmlsavematrices('C:/temp/matdata1.xml',{m1,m2},{'matrix1','matrix2'});
            %%% run ilMatFunTest();
        [matval,matname]=xmlreadmatrices('c:/temp/matdata2.xml');
        edist=eucliddist(m1,m2);
        err=mean(abs(edist(:)-matval{1}(:)))
    */
#endif
}

void ilRGB2Gray(pImage src, pMat dst)
{
    uchar* srcptr = Ptr2D<uchar>(ImageAsMatrix(src));
    uchar* dstptr = Ptr2D<uchar>(dst);
    int w = src->width;
    int w2 = w * 2;
    for (int i = 0; i < (src->width*src->height); ++i)
    {
        int const i3 = i * 3; // one multiplication instead of 3
        dstptr[i] = (uchar)(div((int)srcptr[i3] + (int)srcptr[i3 + 1] + (int)srcptr[i3 + 2], 3).quot);
        // dstptr[i]=(uchar)(div((int)srcptr[i]+(int)srcptr[i+w]+(int)srcptr[i+w2],3).quot);
    }
}

